﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;
using Cambia.CoreLib;
using System.Drawing;

namespace LocalizationTest
{
  class PositionSensor : Sensor
  {
    public PositionSensor()
    {
    }

    public override Measurement PredictForSimulation(WorldState worldState, Map map)
    {
      double sigma = 2;
      double gaussianErrorX = RandomProvider.NextNormal() * sigma; // Math.Sqrt(sigma);
      double gaussianErrorY = RandomProvider.NextNormal() * sigma; // Math.Sqrt(sigma);
      double systematicErrorX = 20;
      double systematicErrorY = 20;

      switch (Globals.selfLocatorProperties.SensorNoise)
      {
        case NoiseType.equal: break; //todo
        case NoiseType.noNoise: gaussianErrorX = 0; gaussianErrorY = 0; systematicErrorX = 0; systematicErrorY = 0; break;
        case NoiseType.gaussianAndSystematic: break;
        case NoiseType.gaussianNoSystematic: systematicErrorX = 0; systematicErrorY = 0; break;
        case NoiseType.noGaussianButSystematic: gaussianErrorX = 0; gaussianErrorY = 0; break;
      }

      if (worldState is WorldState1D)
      {
        PositionMeasurement1D measurement = (PositionMeasurement1D)PredictPerfect(worldState, map);
        return new PositionMeasurement1D(measurement.Position + gaussianErrorX + systematicErrorX, this);
      }
      else if (worldState is WorldState2D)
      {
        PositionMeasurement2D measurement = (PositionMeasurement2D)PredictPerfect(worldState, map);
        return new PositionMeasurement2D(
          new Vector2(
          measurement.Position.x + gaussianErrorX + systematicErrorX,
          measurement.Position.y + gaussianErrorY + systematicErrorY), this);
      }
      else return null;
    }

    public override Measurement PredictPerfect(WorldState worldState, Map map)
    {
      if (worldState is WorldState1D)
      {
        return new PositionMeasurement1D((worldState as WorldState1D).Position, this);
      }

      else if (worldState is WorldState2D)
      {
        return new PositionMeasurement2D(
          new Vector2((worldState as WorldState2D).Vector.x, (worldState as WorldState2D).Vector.y),
          this);
      }
      else
        return null;
    }

    public override double GetMeasurementProbability(WorldState worldState, Map map, Measurement measurement)
    {
      if (worldState is WorldState1D)
      {
        double expectedMeasurement = (PredictPerfect(worldState, map) as PositionMeasurement1D).Position;
        double conductedMeasurement = (measurement as PositionMeasurement1D).Position;
        return Globals.Gaussian(conductedMeasurement, 2, expectedMeasurement);
      }
      else if (worldState is WorldState2D)
      {
        Vector2 expectedMeasurement = (PredictPerfect(worldState, map) as PositionMeasurement2D).Position;
        Vector2 conductedMeasurement = (measurement as PositionMeasurement2D).Position;
        return 
          (
          Globals.Gaussian(conductedMeasurement.x, 20, expectedMeasurement.x) +
          Globals.Gaussian(conductedMeasurement.y, 20, expectedMeasurement.y) 
          ) / 2.0 ;
      }
      else
        return 0;
    }

/*
    public override void  Draw(System.Drawing.Graphics graphics, Measurement measurement, WorldState worldState, double shade)
    {
      double startX = 0;
      double startY = 0;
      double endX = 0;
      double endY = 0;
      if (worldState is WorldState1D)
      {
        startX = (worldState as WorldState1D).Position;
        startY = 0;
        endX = (measurement as PositionMeasurement1D).Position;
        endY = 0;
      }
      if (worldState is WorldState2D)
      {
        startX = (worldState as WorldState2D).Vector.x;
        startY = (worldState as WorldState2D).Vector.y;
        endX = (measurement as PositionMeasurement2D).Position.x;
        endY = (measurement as PositionMeasurement2D).Position.y;
      }

      Color color = Color.FromArgb((int)(255.0), (int)(200.0 * shade), (int)(200.0 * shade));
      Pen pen;
      if (shade == 0.9999)
        pen = new Pen(color, 1);
      else
        pen = new Pen(color, 3);
      graphics.DrawLine(pen, (float)startX, (float)startY, (float)endX, (float)endY);
    }
*/
    public override void Draw(Measurement measurement, WorldState worldState, double shade, string drawingID)
    {
      //TODO
      //throw new Exception("The method or operation is not implemented.");
    }

    public override string ToString()
    {
      throw new Exception("The method or operation is not implemented.");
    }
  }
}
